#!/usr/bin/env python3
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
  # RViZ2 settings
  rviz2_config = os.path.join(
      get_package_share_directory('oradar_lidar'),
      'rviz2',
      'oradar_scan.rviz'
  )

  #Include OrdLidar launch file
  # base_link to lidar tf node
  ordlidar_launch = IncludeLaunchDescription(
      launch_description_source=PythonLaunchDescriptionSource([
          get_package_share_directory('oradar_lidar'),
          '/launch/ms200_scan.launch.py'
      ])
  )
  
  
  
  # base_link to laser_frame tf node
  odom_to_base_link_tf = Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    name='odom_to_base_link_tf',
    arguments=['0','0','0','0','0','0','odom','base_link']
  )
  
  slam_gmapping_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('slam_gmapping'), 'launch'),
         '/slam_gmapping.launch.py'])
    )
  
  # Add cartographer_ros nodes
  cartographer_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('cartographer_ros'), 'launch'),
         '/at_car.launch.py'])
    )
  
  # Add nodes to Launch car_ctrl
  car_launch = Node(
    package='pkg_car_ctrl',
    executable='car_ctrl',
    name='car_ctrl',
    output='screen'
  )

  # Define LaunchDescription variable
  ord = LaunchDescription()

  ord.add_action(ordlidar_launch)
  ord.add_action(odom_to_base_link_tf)
  ord.add_action(cartographer_launch)
  ord.add_action(car_launch)
  
  # ord.add_action(slam_gmapping_launch)
  
  
  return ord